#include <iostream>
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/common.h>
#include <pcl/common/centroid.h>
#include <pcl_ros/transforms.h>
#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>
#include <dynamic_reconfigure/server.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include "lidar_tool/param_extinctConfig.h"
#include "ObjectList.h"
#include "DetectionList.h"
#include <jsk_recognition_msgs/BoundingBoxArray.h>
#include <visualization_msgs/MarkerArray.h>

using namespace std;

Eigen::Affine3f transform0;
Eigen::Affine3f transform1;
Eigen::Affine3f transform2;
Eigen::Affine3f transform3;

ros::NodeHandle &get_handle()
{
    static ros::NodeHandle nh;
    return nh;
}
template <class T>
ros::Publisher &get_publisher(const std::string &topic)
{
    static std::map<std::string, ros::Publisher> publisher_map;
    if (publisher_map.find(topic) == publisher_map.end())
    {
        publisher_map[topic] = get_handle().advertise<T>(topic, 1000);
    }
    return publisher_map[topic];
}

template <typename Point>
void pubpc(std::string topic, pcl::PointCloud<Point> &pc, std::string frame = "base_link")
{
    sensor_msgs::PointCloud2 msgs;
    pcl::toROSMsg(pc, msgs);
    msgs.header.frame_id = frame;
    get_publisher<sensor_msgs::PointCloud2>(topic).publish(msgs);
}

static void Callbackdynamic(lidar_tool::param_extinctConfig &config)
{
    transform0 = Eigen::Affine3f::Identity();
    transform0.translation() << config.x0, config.y0, config.z0;
    transform0.rotate(Eigen::AngleAxisf(config.yaw0, Eigen::Vector3f::UnitZ()));
    transform0.rotate(Eigen::AngleAxisf(config.pitch0, Eigen::Vector3f::UnitY()));
    transform0.rotate(Eigen::AngleAxisf(config.roll0, Eigen::Vector3f::UnitX()));

    transform1 = Eigen::Affine3f::Identity();
    transform1.translation() << config.x1, config.y1, config.z1;
    transform1.rotate(Eigen::AngleAxisf(config.yaw1, Eigen::Vector3f::UnitZ()));
    transform1.rotate(Eigen::AngleAxisf(config.pitch1, Eigen::Vector3f::UnitY()));
    transform1.rotate(Eigen::AngleAxisf(config.roll1, Eigen::Vector3f::UnitX()));

    transform2 = Eigen::Affine3f::Identity();
    transform2.translation() << config.x2, config.y2, config.z2;
    transform2.rotate(Eigen::AngleAxisf(config.yaw2, Eigen::Vector3f::UnitZ()));
    transform2.rotate(Eigen::AngleAxisf(config.pitch2, Eigen::Vector3f::UnitY()));
    transform2.rotate(Eigen::AngleAxisf(config.roll2, Eigen::Vector3f::UnitX()));

    transform3 = Eigen::Affine3f::Identity();
    transform3.translation() << config.x3, config.y3, config.z3;
    transform3.rotate(Eigen::AngleAxisf(config.yaw3, Eigen::Vector3f::UnitZ()));
    transform3.rotate(Eigen::AngleAxisf(config.pitch3, Eigen::Vector3f::UnitY()));
    transform3.rotate(Eigen::AngleAxisf(config.roll3, Eigen::Vector3f::UnitX()));

    std::cout<<"transform0\n"<<transform0.matrix()<<std::endl;
    std::cout<<"transform1\n"<<transform1.matrix()<<std::endl;
    std::cout<<"transform2\n"<<transform2.matrix()<<std::endl;
    std::cout<<"transform3\n"<<transform3.matrix()<<std::endl;
}

void velodyne_callback(const sensor_msgs::PointCloud2ConstPtr &cloud)
{
    pcl::PointCloud<pcl::PointXYZI> cloud1;
    pcl::fromROSMsg(*cloud,cloud1);
    pcl::transformPointCloud(cloud1, cloud1, transform0);
    pubpc("lidar", cloud1);
}

void radar_callback3(const sensor_msgs::PointCloud2ConstPtr &cloud)
{
    pcl::PointCloud<pcl::PointXYZI> cloud1;
    pcl::fromROSMsg(*cloud,cloud1);
    pcl::transformPointCloud(cloud1, cloud1, transform1);

    pubpc("radar", cloud1);
}

void radar_callback(const ars548_msg::ObjectListConstPtr &cloud)
{
    uint size = cloud->object_array.size();
    visualization_msgs::Marker my_marker;
    visualization_msgs::MarkerArray marker_array;
    marker_array.markers.clear();

    if (size > 0) {
        for (uint i = 0; i < size; i++) {
            my_marker.header.frame_id = "base_link";
            my_marker.header.stamp = cloud->object_array[i].header.stamp;
            my_marker.ns = "object_shapes";

            my_marker.id = cloud->object_array[i].u_ID;
            my_marker.type = visualization_msgs::Marker::CUBE;
            my_marker.action = visualization_msgs::Marker::ADD;

            pcl::PointCloud<pcl::PointXYZI> cloud1;
            pcl::PointXYZI pt;
            pt.x = cloud->object_array[i].u_Position_X;
            pt.y = cloud->object_array[i].u_Position_Y;
            pt.z = cloud->object_array[i].u_Position_Z;
            cloud1.push_back(pt);
            pcl::transformPointCloud(cloud1, cloud1, transform1);

            my_marker.pose.position.x = cloud1[0].x;
            my_marker.pose.position.y = cloud1[0].y;
            my_marker.pose.position.z = cloud1[0].z;

            my_marker.pose.orientation.x = 0.0;
            my_marker.pose.orientation.y = 0.0;
            my_marker.pose.orientation.z = sin(cloud->object_array[i].u_Position_Orientation / 2);
            my_marker.pose.orientation.w = cos(cloud->object_array[i].u_Position_Orientation / 2);

            if ((cloud->object_array[i].u_Shape_Length_Edge_Mean > 0.2) ||
                (cloud->object_array[i].u_Shape_Width_Edge_Mean > 0.2)) {
                my_marker.scale.x = cloud->object_array[i].u_Shape_Length_Edge_Mean;
                my_marker.scale.y = cloud->object_array[i].u_Shape_Width_Edge_Mean;
                my_marker.scale.z = (cloud->object_array[i].u_Shape_Length_Edge_Mean +
                                     cloud->object_array[i].u_Shape_Width_Edge_Mean) / 2;
            } else {
                my_marker.scale.x = 0.2;
                my_marker.scale.y = 0.2;
                my_marker.scale.z = 0.2;
            }

            my_marker.color.r = 0.0f;
            my_marker.color.g = 1.0f;
            my_marker.color.b = 0.0f;
            my_marker.color.a = 1.0;

            my_marker.lifetime = ros::Duration(0.5);

            marker_array.markers.push_back(my_marker);
        }
        get_publisher<visualization_msgs::MarkerArray>("radar").publish(marker_array);
    }
}


void radar_callback2(const ars548_msg::DetectionListConstPtr &cloud)
{
    uint size = cloud->detection_array.size();
    pcl::PointCloud<pcl::PointXYZI> cloud1;

    if (size > 0) {
        for (uint i = 0; i < size; i++) {
            pcl::PointXYZI pt;
            pt.x = cloud->detection_array[i].f_x;
            pt.y = cloud->detection_array[i].f_y;
            pt.z = cloud->detection_array[i].f_z;
            cloud1.push_back(pt);
        }
        pcl::transformPointCloud(cloud1, cloud1, transform1);

        sensor_msgs::PointCloud2 msgs;
        pcl::toROSMsg(cloud1, msgs);
        msgs.header.frame_id = "base_link";
        get_publisher<sensor_msgs::PointCloud2>("radar_pc").publish(msgs);
    }
}

pcl::PointCloud<pcl::PointXYZI>::Ptr cloud0(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud3(new pcl::PointCloud<pcl::PointXYZI>);

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "test_node");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/pointcloud_lidar3", 10, velodyne_callback);
    ros::Subscriber sub2 = nh.subscribe("/radar_obj_list", 10, radar_callback);
    ros::Subscriber sub3 = nh.subscribe("/radar_det_list", 10, radar_callback2);
    ros::Subscriber sub4 = nh.subscribe("/pointcloud_radar3", 10, radar_callback3);

    dynamic_reconfigure::Server<lidar_tool::param_extinctConfig> server;
    dynamic_reconfigure::Server<lidar_tool::param_extinctConfig>::CallbackType f;
    f = boost::bind(&Callbackdynamic, _1); //绑定回调函数
    server.setCallback(f);

    ros::spin();
    return 0;
}
